#pragma once
#include <Eigen/Core>
#include "graph_optimizer/utility/data_types.hpp"
namespace graph_optimizer
{
    class PreIntegration
    {
    public:
        PreIntegration() = delete;
        PreIntegration(const std::deque<OgData> &og_data, const Eigen::Vector3d &linearized_bg)
            : og_data_(og_data), linearized_bg_(linearized_bg),
              jacobian_(Eigen::Matrix<double, 9, 9>::Identity()), covariance_(Eigen::Matrix<double, 9, 9>::Zero())
        {
            noise_ = Eigen::Matrix<double, 12, 12>::Zero();
            noise_.block<3, 3>(0, 0) = (phi_noise * phi_noise) * Eigen::Matrix3d::Identity();
            noise_.block<3, 3>(3, 3) = (gyro_noise * gyro_noise) * Eigen::Matrix3d::Identity();
            noise_.block<3, 3>(6, 6) = (gyro_noise * gyro_noise) * Eigen::Matrix3d::Identity();
            noise_.block<3, 3>(9, 9) = (bg_noise * bg_noise) * Eigen::Matrix3d::Identity();
            alpha_.setZero();
            dq_.setIdentity();
        }

        void repropagate(const Eigen::Vector3d &linearized_bg)
        {
            alpha_.setZero();
            dq_.setIdentity();
            linearized_bg_ = linearized_bg;
            jacobian_.setIdentity();
            covariance_.setZero();
            propagate();
        }

        void midPointIntegration(double dt,
                                 const Eigen::Vector3d &phi0, const Eigen::Vector3d &gyr0,
                                 const Eigen::Vector3d &gyr1,
                                 const Eigen::Vector3d &alpha, const Eigen::Quaterniond &dq,
                                 const Eigen::Vector3d &linearized_bg,
                                 Eigen::Vector3d &result_alpha, Eigen::Quaterniond &result_dq, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
        {
            Eigen::Vector3d gyro_av = 0.5 * (gyr0 + gyr1) - linearized_bg;
            Eigen::Quaterniond temp = Eigen::Quaterniond(1, gyro_av(0) * dt / 2, gyro_av(1) * dt / 2, gyro_av(2) * dt / 2);
            temp.normalize();
            result_dq = dq * temp;
            result_alpha = alpha + dq * phi0 * dt / 0.01;
            result_linearized_bg = linearized_bg;

            if (update_jacobian)
            {
                Eigen::Matrix3d R_w_x, R_phi0_x;

                R_w_x << 0, -gyro_av(2), gyro_av(1),
                    gyro_av(2), 0, -gyro_av(0),
                    -gyro_av(1), gyro_av(0), 0;
                R_phi0_x << 0, -phi0(2), phi0(1),
                    phi0(2), 0, -phi0(0),
                    -phi0(1), phi0(0), 0;

                Eigen::Matrix<double, 9, 9> F = Eigen::Matrix<double, 9, 9>::Zero();
                F.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
                F.block<3, 3>(0, 3) = -dq.toRotationMatrix() * R_phi0_x * (dt / 0.01);

                F.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() - R_w_x * dt;
                F.block<3, 3>(3, 6) = -1.0 * dt * Eigen::Matrix3d::Identity(3, 3);
                F.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity();

                Eigen::Matrix<double, 9, 12> G = Eigen::Matrix<double, 9, 12>::Zero();
                G.block<3, 3>(0, 0) = dq.toRotationMatrix() * (dt / 0.01);
                G.block<3, 3>(3, 3) = 0.5 * dt * Eigen::Matrix3d::Identity(3, 3);
                G.block<3, 3>(3, 6) = 0.5 * dt * Eigen::Matrix3d::Identity(3, 3);
                G.block<3, 3>(6, 9) = Eigen::Matrix3d::Identity(3, 3) * dt;

                jacobian_ = F * jacobian_;
                covariance_ = F * covariance_ * F.transpose() + G * noise_ * G.transpose();
            }
        }

        void propagate()
        {
            for (int i = 1; i < static_cast<int>(og_data_.size()); ++i)
            {
                double dt = og_data_[i].time - og_data_[i - 1].time;
                Eigen::Vector3d phi0 = og_data_[i - 1].phi;
                Eigen::Vector3d gry0 = og_data_[i - 1].gyro;
                Eigen::Vector3d gry1 = og_data_[i].gyro;
                Eigen::Vector3d result_alpha;
                Eigen::Quaterniond result_dq;
                Eigen::Vector3d result_linearized_bg;
                midPointIntegration(dt, phi0, gry0, gry1, alpha_, dq_,
                                    linearized_bg_, result_alpha, result_dq, result_linearized_bg, true);
                alpha_ = result_alpha;
                dq_ = result_dq;
                linearized_bg_ = result_linearized_bg;
                dq_.normalize();
            }
        }

        Eigen::Matrix<double, 9, 1> evaluate(const Eigen::Vector3d &Pi,
                                             const Eigen::Quaterniond &Qi,
                                             const Eigen::Vector3d &Bgi,
                                             const Eigen::Vector3d &Pj,
                                             const Eigen::Quaterniond &Qj,
                                             const Eigen::Vector3d &Bgj)
        {
            Eigen::Matrix<double, 9, 1> residuals;

            Eigen::Vector3d dbg = Bgi - linearized_bg_;
            Eigen::Matrix3d dalpha_dbg = jacobian_.block<3, 3>(0, 6);
            Eigen::Matrix3d dq_dbg = jacobian_.block<3, 3>(3, 6);

            Eigen::Quaterniond corrected_dq = dq_ * deltaQ(dq_dbg * dbg);
            Eigen::Vector3d corrected_alpha = alpha_ + dalpha_dbg * dbg;

            residuals.block<3, 1>(0, 0) = Qi.inverse() * (Pj - Pi) - corrected_alpha;
            residuals.block<3, 1>(3, 0) = 2 * (corrected_dq.inverse() * (Qi.inverse() * Qj)).vec();
            residuals.block<3, 1>(6, 0) = Bgj - Bgi;
            return residuals;
        }

        Eigen::Matrix<double, 9, 18> jacobian(const Eigen::Vector3d &Pi,
                                              const Eigen::Quaterniond &Qi,
                                              const Eigen::Vector3d &Bgi,
                                              const Eigen::Vector3d &Pj,
                                              const Eigen::Quaterniond &Qj,
                                              const Eigen::Vector3d &Bgj)
        {
            Eigen::Matrix<double, 9, 18> jacobians;
            jacobians.setZero();

            Eigen::Vector3d dbg = Bgi - linearized_bg_;
            Eigen::Matrix3d dalpha_dbg = jacobian_.block<3, 3>(0, 6);
            Eigen::Matrix3d dq_dbg = jacobian_.block<3, 3>(3, 6);
            Eigen::Quaterniond corrected_dq = dq_ * deltaQ(dq_dbg * dbg);
            // Eigen::Vector3d corrected_alpha = alpha_ + dalpha_dbg * dbg;

            jacobians.block<3, 3>(0, 0) = -Qi.inverse().toRotationMatrix();
            jacobians.block<3, 3>(0, 3) = skewSymmetric(Qi.inverse() * (Pj - Pi));
            jacobians.block<3, 3>(0, 6) = -dalpha_dbg;

            jacobians.block<3, 3>(0, 9) = Qi.inverse().toRotationMatrix();

            jacobians.block<3, 3>(3, 3) = -(Qleft(Qj.inverse() * Qi) * Qright(corrected_dq)).bottomRightCorner<3, 3>();
            jacobians.block<3, 3>(3, 6) = -(Qleft(Qj.inverse() * Qi * corrected_dq)).bottomRightCorner<3, 3>() * dq_dbg;

            jacobians.block<3, 3>(3, 12) = (Qleft(corrected_dq.inverse() * Qi.inverse() * Qj)).bottomRightCorner<3, 3>();

            jacobians.block<3, 3>(6, 6) = -Eigen::Matrix3d::Identity();
            jacobians.block<3, 3>(6, 15) = Eigen::Matrix3d::Identity();
            return jacobians;
        }

        std::deque<OgData> og_data_;
        Eigen::Vector3d linearized_bg_;

        Eigen::Matrix<double, 9, 9> jacobian_, covariance_;
        Eigen::Matrix<double, 12, 12> noise_;

        Eigen::Vector3d alpha_;
        Eigen::Quaterniond dq_;
    };
} // namespace graph_optimizer